#!/usr/bin/env python3

import rospy
import tf
from geometry_msgs.msg import PoseStamped, Point


def pose_publisher():
    pose_publisher = rospy.Publisher('/robot_pose', PoseStamped, queue_size=10)
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        tf_listener = tf.TransformListener()
        tf_listener.waitForTransform('/odom', '/base_link', rospy.Time(0), rospy.Duration(60.0))
        trans, _ = tf_listener.lookupTransform('/odom', '/base_link', rospy.Time(0))
        robot_pose = Point(*trans)

        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.header.stamp = rospy.Time.now()
        pose.pose.position.x = robot_pose.x
        pose.pose.position.y = robot_pose.y
        pose.pose.position.z = robot_pose.z
        rospy.loginfo("\r\nrobot position \
                      \r\n[x: %.2f, y: %.2f, z: %.2f]" %
                      (robot_pose.x, robot_pose.y, robot_pose.z))

        pose_publisher.publish(pose)
        rate.sleep()


if __name__ == '__main__':
    try:
        rospy.init_node('pose_publisher', anonymous=True)
        pose_publisher()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
